#include <mutex>
#include <thread>
#include "../include/Agente.h"
#include "../../Logger/include/Logger.h"
#include "../../Matematica/include/Matematica.h"



void
Agente::setHead(Matriz4d head){

	mAgentTransformHead =head;

}

void
Agente::setBody(Matriz4d body){

	  mAgentTransformBody = body;

}

void
Agente::setRmupperarm(Matriz4d mupperarmR){

    mAgentTransformRmupperarm = mupperarmR;
}
void
Agente::setRlowerarm(Matriz4d lowerarmR){

	 mAgentTransformRlowerarm = lowerarmR;

}

void
Agente::setLupperarm(Matriz4d upperarmL){

     mAgentTransformLupperarm = upperarmL;
}
void
Agente::setLlowerarm(Matriz4d lowerarmL){

	 mAgentTransformLlowerarm = lowerarmL;

}

void
Agente::setRthigh(Matriz4d thighR){

	  mAgentTransformRthigh = thighR;

}
void
Agente::setRshank(Matriz4d shankR){

      mAgentTransformRshank = shankR;
}
void
Agente::setRfoot(Matriz4d footR ){

       mAgentTransformRfoot = footR;
}

void
Agente::setLthigh(Matriz4d thighL){

   mAgentTransformLthigh = thighL;
}
void
Agente::setLshank(Matriz4d shankL){

	  mAgentTransformLshank = shankL;

}
void
Agente::setLfoot(Matriz4d footL){

     mAgentTransformLfoot = footL;
}

void
Agente::setBoll(Matriz4d boll){

  mBallTransformAgente = boll;
}


Coordenada3d Agente::getHeadAgentePosition(){//posicao da cabeça

	std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);

     return mAgentTransformHead.getPosition();

}

Coordenada3d Agente::getBodyAgentePosition(){//posicao do corpo

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformBody.getPosition();

}

Coordenada3d Agente::getRmupperarmAgentePosition(){//braço superior direito

	std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);

    return mAgentTransformRmupperarm.getPosition();

}

Coordenada3d Agente::getRlowerarmAgentePosition(){//ante-braço direito

    std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformRlowerarm.getPosition();

}

Coordenada3d Agente::getLmupperarmAgentePosition(){//braço superior esquerdo

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformLupperarm.getPosition();

}

Coordenada3d Agente::getLlowerarmAgentePosition(){//ante-braço esquerdo


	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformLlowerarm.getPosition();


}


Coordenada3d Agente::getRthighAgentePosition(){ //coixa direita

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

    return mAgentTransformRthigh.getPosition();
}

Coordenada3d Agente::getRshankAgentePosition(){//perna direita

	std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);

    return mAgentTransformRshank.getPosition();
}

Coordenada3d Agente::getRfootAgentePosition(){//pe direito

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformRfoot.getPosition();
}


Coordenada3d Agente::getLthighAgentePosition(){ //coixa esquerda

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

    return mAgentTransformLthigh.getPosition();
}

Coordenada3d Agente::getLshankAgentePosition(){//perna esquerda

	std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);

    return mAgentTransformLshank.getPosition();
}

Coordenada3d Agente::getLfootAgentePosition(){//pe esquerdo

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mAgentTransformLfoot.getPosition();
}

Matriz4d Agente:: getParteAgente(int valor){

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	Matriz4d parte;

	switch(valor){

	case 1:
		parte = mAgentTransformHead;
		break;
	case 2:

		parte = mAgentTransformBody;
	    break;
	case 3:
		parte = mAgentTransformRmupperarm;
	    break;
	case 4:
		parte = mAgentTransformRlowerarm;
	    break;

	case 5:
		parte = mAgentTransformLupperarm;
		break;
	case 6:
		parte = mAgentTransformLlowerarm;
	    break;

	case 7:
		parte = mAgentTransformRthigh;
	    break;
	case 8:
		parte = mAgentTransformRshank;
		break;
	case 9:
		parte = mAgentTransformRfoot;
	    break;
	case 10:
		parte = mAgentTransformLthigh;
		break;
	case 11:
		parte = mAgentTransformLshank;
		break;
	case 12:
		parte = mAgentTransformLfoot;
		break;



	}


	return parte;
}

Matriz4d Agente::getBollM4(){

	return mBallTransformAgente;
}

void Agente::addPosicoesAgente(Agente agentPosi){

	  ListPosicao.push_back(agentPosi);

}

void Agente::setTime(double time){

	mTime = time;
}


void Agente::setTimCaido(double tem){

	tempoCaido=tempoCaido+tem;
}

void Agente::setTimInici(double tim){

	dtempIni=tim;
}

void Agente::setPosseBola(bool val){

	posseBola = val;

}



void Agente::setNumeroReal(int num){

	mNumero = num;

}

void Agente::setLado(std::string  lad){

	lado =lad;
}
void Agente::setNumero(std::string sNum){

	if(sNum=="matNum11")
		  mNumero=11;
	if(sNum=="matNum10")
		  mNumero=10;
	if(sNum=="matNum9")
		  mNumero=9;
	if(sNum=="matNum9")
		  mNumero=9;
	if(sNum=="matNum8")
		  mNumero=8;
	if(sNum=="matNum7")
		  mNumero=7;
	if(sNum=="matNum6")
		  mNumero=6;
	if(sNum=="matNum5")
		  mNumero=5;
	if(sNum=="matNum4")
		  mNumero=4;
	if(sNum=="matNum3")
		  mNumero=3;
	if(sNum=="matNum2")
		  mNumero=2;
	if(sNum=="matNum1")
		  mNumero=1;



}

void Agente::setPlayMode(std::string play){





	 if(play=="0")
		 play= "BeforeKickOff";


     if(play=="1")
		 play= "KickOff_Left";


	if(play=="2")
	 	  play= "KickOff_Right";


	if(play=="3")
	 	  play= "PlayOn";

	if(play=="4")
	 	  play= "KickIn_Left";

	if(play=="5")
	 	  play= "KickIn_Right";

	if(play=="6")
	 	  play= "corner_kick_left";

	if(play=="7")
	 	  play= "corner_kick_right";

	if(play=="8")
	 	  play= "goal_kick_left";

	if(play=="9")
	 	  play= "goal_kick_right";

	if(play=="10")
	 	  play= "offside_left";

	if(play=="11")
		 	  play= "offside_right";

	if(play=="12")
		 	  play= "GameOver";

	if(play=="13")
		 	  play= "Goal_Left";

	if(play=="14")
		 	  play= "Goal_Right";

	if(play=="15")
		 	  play= "free_kick_left";

	if(play=="16")
		 	  play= "free_kick_right";


	  playMode = play;

}

void Agente::contQuedas(){

	  iqutQuedas++;


}

double Agente::getTemcaido(){
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return tempoCaido;
}

double Agente::getTemInici(){
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return dtempIni;
}

void Agente::setStatus(bool sta){
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	bcaido = sta;
}



bool Agente::getStatus(){

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return bcaido;
}



/*Retorna dados*/

void Agente::imprimNum(){

	std::cout<<"Numero do agente: "<<mNumero<<"\n";

}


int Agente::getNumero(){
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

    return mNumero;
}

std::string  Agente:: getLado(){

	std::mutex mutex;
    std::lock_guard<std::mutex> lock(mutex);
	std::string desc;

	if(lado=="matLeft") desc="Left";
	if(lado=="matRight") desc="Right";
    return desc;
}


double Agente::getTimeAgene(){
	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mTime;

}

int Agente::getQuedas(){

	return iqutQuedas;
}

bool Agente::getPosseBola(){

	return posseBola;
}

Coordenada3d  Agente::getBollAgente(){

	std::mutex mutex;
	std::lock_guard<std::mutex> lock(mutex);

	return mBallTransformAgente.getPosition();

}

std::string Agente::getPlayMode(){

	return playMode;
}

void Agente::tostringPartAgent(Coordenada3d parte3D){


   std::cout<<"X:"<<parte3D.X()<<" Y:"<<parte3D.Y()<<" Z:"<<parte3D.Z()<<std::endl;
}

void Agente::ListaCoordenadaCorpo(){

	//std::cout<<"============================================================================================================"<<std::endl;
	std::cout<<"Time:"<<mTime<<std::endl;
    std::cout<<"Coordenada corpo do agente: "<<mAgentTransformBody.getPosition().X()<<" "<<mAgentTransformBody.getPosition().Y()<<" "<<mAgentTransformBody.getPosition().Z()<<" ";

}


std::vector<Agente>  Agente::getVetorPosicoes(){


	return ListPosicao;


}
void Agente::getPosicoesAgente(){




    for(i=ListPosicao.begin(); i!=ListPosicao.end(); i++){

             ((Agente)*i).ListaCoordenadaCorpo();
    }



}

